#!/usr/bin/env python
import rospy
import tf
import transforms3d as tfs
import geometry_msgs.msg
import sys
import numpy as np
import transforms3d as tfs
import math
from tf2_msgs.msg import TFMessage
  



if __name__ == '__main__':
    rospy.init_node('turtle_tf_listener')   

    listener = tf.TransformListener()
    br = tf.TransformBroadcaster()  

    rate = rospy.Rate(1.0)
    while not rospy.is_shutdown():
        try:
            (trans2,rot2) = listener.lookupTransform('/camera_frame', '/aruco_marker_frame', rospy.Time(0))


            # aruco_marker_frame = 7_Link ,so aruco_marker_frame to camera = 7_Link to mycamera
            (trans2_inv,rot2_inv) = listener.lookupTransform( '/aruco_marker_frame', '/camera_frame',rospy.Time(0))
            br.sendTransform(trans2_inv,rot2_inv,rospy.Time.now(),"my_camera","7_Link")
            # my_camera to my_marker
            br.sendTransform(trans2,rot2,rospy.Time.now(),"my_marker","my_camera")


            (trans1,rot1) = listener.lookupTransform('/base_link', '/my_camera', rospy.Time(0))
            print("base_link->my_camera",trans1,rot1)
            print("my_camera->my_marker",trans2,rot2)

        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        rate.sleep()
